'''
控制机器人执行以下动作序列：
1. 恢复站立
2. 快速小跑5秒
3. 向右转并继续快速小跑2秒
4. 急停
'''
import lcm
import sys
import time
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt

def main():
    # 初始化机器人控制对象
    Ctrl = Robot_Ctrl()
    # 启动机器人控制
    Ctrl.run()
    # 创建控制命令消息
    msg = robot_control_cmd_lcmt()
    
    try:
        # 1. 恢复站立
        msg.mode = 12  # Recovery stand
        msg.gait_id = 0
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(12, 0)
        
        # 2. 快速小跑3秒
        msg.mode = 11  # Locomotion
        msg.gait_id = 10  # TROT_FAST
        msg.vel_des = [0.5, 0, 0]  # 向前运动
        msg.duration = 0  # 持续运动
        msg.step_height = [0.06, 0.06]  # 摆动腿高度
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(3)  # 运行5秒
        
        # 3. 向右转并继续快速小跑2秒
        msg.mode = 11  # Locomotion
        msg.gait_id = 10  # TROT_FAST
        msg.vel_des = [0, 0, -1]  # 纯向右
        msg.duration = 0  # 持续运动
        msg.step_height = [0.06, 0.06]  # 摆动腿高度
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(2)  # 运行2秒
        
        # 4. 急停（使用纯阻尼模式实现快速停止）
        msg.mode = 7  # PureDamper
        msg.gait_id = 0
        msg.vel_des = [0, 0, 0]
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.Wait_finish(7, 0)

    except KeyboardInterrupt:
        # 捕获键盘中断，安全退出
        msg.mode = 7  # PureDamper
        msg.gait_id = 0
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        pass
    
    # 退出程序
    Ctrl.quit()
    sys.exit()

class Robot_Ctrl(object):
    def __init__(self):
        self.rec_thread = Thread(target=self.rec_responce)
        self.send_thread = Thread(target=self.send_publish)
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        self.cmd_msg = robot_control_cmd_lcmt()
        self.rec_msg = robot_control_response_lcmt()
        self.send_lock = Lock()
        self.delay_cnt = 0
        self.mode_ok = 0
        self.gait_ok = 0
        self.runing = 1

    def run(self):
        self.lc_r.subscribe("robot_control_response", self.msg_handler)
        self.send_thread.start()
        self.rec_thread.start()

    def msg_handler(self, channel, data):
        self.rec_msg = robot_control_response_lcmt().decode(data)
        if self.rec_msg.order_process_bar >= 95:
            self.mode_ok = self.rec_msg.mode
        else:
            self.mode_ok = 0

    def rec_responce(self):
        while self.runing:
            self.lc_r.handle()
            time.sleep(0.002)

    def Wait_finish(self, mode, gait_id):
        count = 0
        while self.runing and count < 2000:
            if self.mode_ok == mode and self.gait_ok == gait_id:
                return True
            else:
                time.sleep(0.005)
                count += 1
        return False

    def send_publish(self):
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20:
                self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode())
                self.delay_cnt = 0
            self.delay_cnt += 1
            self.send_lock.release()
            time.sleep(0.005)

    def Send_cmd(self, msg):
        self.send_lock.acquire()
        self.delay_cnt = 50
        self.cmd_msg = msg
        self.send_lock.release()

    def quit(self):
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()

if __name__ == '__main__':
    main() 